#include "ros/ros.h"
#include "turtle_move/move0.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"

double old_x, old_y;
double new_x, new_y;
void getOLdPose(const turtlesim::Pose::ConstPtr &p)
{
    old_x = p->x;
    old_y = p->y;
}

void getNewPose(const turtlesim::Pose::ConstPtr &p)
{
    new_x = p->x;
    new_y = p->y;
}

bool doReq(turtle_move::move0::Request &req,
           turtle_move::move0::Response &resp)
{
    ros::Rate r(1);

    ros::NodeHandle nh;
    ros::Subscriber sub1 =
        nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1, getOLdPose);
    r.sleep();
    ros::spinOnce();

    double tar_x = req.x;
    double tar_y = req.y;

    ROS_INFO("当前坐标为:x = %lf, y = %lf", old_x, old_y);
    ROS_INFO("目标坐标为:x = %lf, y = %lf", tar_x, tar_y);
    ROS_INFO("即将开始移动......");

    double mov_x = tar_x - old_x;
    double mov_y = tar_y - old_y;
    geometry_msgs::Twist msg;
    msg.linear.x = mov_x;
    msg.linear.y = mov_y;
    msg.linear.z = 0.0;

    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 0.0;

    ros::Publisher pub =
        nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1000);
    r.sleep();
    pub.publish(msg);

    ros::Subscriber sub2 =
        nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1, getNewPose);
    r.sleep();
    ros::spinOnce();

    ROS_INFO("运动结束,当前坐标为:x = %lf, y = %lf", new_x, new_y);
    resp.x = new_x;
    resp.y = new_y;

    return true;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "Move_Server0");

    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("Move_Server0", doReq);
    ros::spin();
    ROS_INFO("服务已经启动....");

    return 0;
}
